import java.awt.BorderLayout;
import java.awt.Color;
import java.awt.Dimension;
import java.awt.Graphics;
import java.awt.Graphics2D;
import java.awt.event.ActionEvent;
import java.awt.event.ActionListener;
import java.awt.geom.Ellipse2D;
import java.awt.geom.Line2D;
import java.awt.geom.Point2D;
import java.io.File;
import java.io.FileNotFoundException;
import java.util.InputMismatchException;
import java.util.LinkedList;
import java.util.Random;
import java.util.Scanner;

import javax.swing.JButton;
import javax.swing.JFileChooser;
import javax.swing.JFrame;
import javax.swing.JMenu;
import javax.swing.JMenuBar;
import javax.swing.JMenuItem;
import javax.swing.JPanel;
import javax.swing.JLabel;
import javax.swing.JComboBox;


public class RRTree extends JPanel implements ActionListener {

	private class RRTreeNode{
		protected RRTreeNode parent;
		protected LinkedList <RRTreeNode> children;
		protected Point2D position;
		protected double orientation;
		
		public RRTreeNode(){
			parent=null;
			children=new LinkedList <RRTreeNode>();
			position=null;
		}
		public RRTreeNode(RRTreeNode _parent, LinkedList <RRTreeNode> _children, Point2D _position, double _orientation){
			parent=_parent;
			children=_children;
			position=_position;
			orientation=_orientation;
		}
		public String toString(){
			String s = "";
			s += "Pos=<";
			s += position;
			s += "> Orientation=<";
			s += orientation;
			s += ">";
			return s;
		}
		public boolean equals(RRTreeNode b){
			return this.position==b.position; // may want to change this later so that orientation is also considered
		}
	}
	
	private JButton start;
	private JButton showpathbutton;
	private JComboBox mulist;
	private RRTreeNode startroot;
	private RRTreeNode goalroot;
	private LinkedList <Obstacle> obstacles;
	private int num_obstacles;
	private LinkedList <RRTreeNode> path;
	private boolean showpath;
	public static double mu = 20;
	public static double mu_orientation = 5;
	
	
	public RRTree(){
		startroot = null;
		goalroot = null;
		obstacles = null;
		num_obstacles = 0;
		path = null;
		showpath = false;
		
		JMenuBar menubar = new JMenuBar();
		JMenu filemenu = new JMenu("File");
		JMenuItem menuitem1 = new JMenuItem("Load start and goal file");
		menuitem1.addActionListener(this);
		JMenuItem menuitem2 = new JMenuItem("Load obstacle file");
		menuitem2.addActionListener(this);
    	JMenuItem menuitem3 = new JMenuItem("Exit");
    	menuitem3.addActionListener(this);
    	filemenu.add( menuitem1 );
    	filemenu.add( menuitem2 );
    	filemenu.add( menuitem3 );
    	menubar.add( filemenu );
    	
    	this.setSize(new Dimension(600,600));
    	
    	start = new JButton("start");
    	start.setEnabled(true);
		start.setActionCommand("start");
		start.addActionListener(this);
		
		showpathbutton = new JButton("show path");
		showpathbutton.setEnabled(false);
		showpathbutton.setActionCommand("showpath");
		showpathbutton.addActionListener(this);

		String[] muvalues = {"5","10","20","50"};
		mulist = new JComboBox(muvalues);
		mulist.setSelectedItem("20");
		mulist.setEnabled(false);
		mulist.setActionCommand("mulist");
		mulist.addActionListener(this);
		
		JPanel panel = new JPanel();
		panel.add(start);
		panel.add(showpathbutton);
		panel.add(new JLabel("  mu param.:"));
    	panel.add(mulist);
		
		JFrame frame = new JFrame("RRTree");
		frame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
		frame.setSize(this.getWidth()+10, this.getHeight()+85);
		frame.add(menubar,BorderLayout.NORTH);
		frame.add(this,BorderLayout.CENTER);
		frame.add(panel,BorderLayout.SOUTH);
		frame.setVisible(true);
	}
    
	public void paint(Graphics g){
		Graphics2D g2 = (Graphics2D)g;
		int w = this.getWidth();
        int h = this.getHeight();
        g2.setColor(Color.lightGray);
        g2.fillRect(0, 0, w, h);
        if (obstacles!=null){
        	for (int k = 0; k < num_obstacles; k++){
        		Obstacle obst = obstacles.get(k);
        		if (! obst.isWall()) {
        			g2.setColor(Color.WHITE);
        			g2.fill(obst.getPoly());
        		}else{}
        		g2.setColor(Color.BLACK);
    			g2.draw(obst.getPoly());
        	}
        }
        paintRRTree(startroot,g2,Color.ORANGE,Color.ORANGE);
        paintRRTree(goalroot,g2,Color.GREEN,Color.GREEN);
        if (showpath){
        	RRTreeNode n1 = path.get(0);
        	g2.setColor(Color.CYAN);
        	Ellipse2D e = new Ellipse2D.Double(n1.position.getX()-2.5,n1.position.getY()-2.5,5,5);
	    	g2.fill(e);
        	RRTreeNode n2;
        	for (int k = 1; k < path.size(); k++){
        		n2 = path.get(k);
        		e = new Ellipse2D.Double(n2.position.getX()-2.5,n2.position.getY()-2.5,5,5);
    	    	g2.fill(e);
            	Line2D l = new Line2D.Double(n1.position,n2.position);
            	n1 = n2;
        	}
        }
	}
	
	private void paintRRTree(RRTreeNode current, Graphics2D g2, Color linecolor, Color pointcolor){
		if (current == null)
			return;
		else{
			if (current.parent!=null){
				g2.setColor(linecolor);
				Line2D l = new Line2D.Double(current.parent.position,current.position);
				g2.draw(l);
				g2.setColor(pointcolor);
			}
			else{
				g2.setColor(Color.RED);
			}
			Ellipse2D e = new Ellipse2D.Double(current.position.getX()-2.5,current.position.getY()-2.5,5,5);
	    	g2.fill(e);
			for (RRTreeNode child : current.children)
				paintRRTree(child, g2, linecolor, pointcolor);
		}
	}
	
	public void actionPerformed(ActionEvent e){
		if ("Load start and goal file".equals(e.getActionCommand())){
			JFileChooser chooser = new JFileChooser(".");
	        if (chooser.showOpenDialog(this)==JFileChooser.APPROVE_OPTION){
	        	String start_goal_file = chooser.getSelectedFile().getName();
	        	loadStartGoalFile(start_goal_file);
	        	clearTrees();
	        	path = null;
	        	showpath = false;
	        }
	        else System.err.println("Problem choosing file");
		}
		else if ("Load obstacle file".equals(e.getActionCommand())){
			JFileChooser chooser = new JFileChooser(".");
			if (chooser.showOpenDialog(this)==JFileChooser.APPROVE_OPTION){
	        	String start_goal_file = chooser.getSelectedFile().getName();
	        	loadObstacleFile(start_goal_file);
	        	clearTrees();
	        	path = null;
	        	showpath = false;
	        }
	        else System.err.println("Problem choosing file");
		}
		else if ("Exit".equals(e.getActionCommand())){
			System.out.println("Goodbye!");
			System.exit(0);
		}
		else if ("start".equals(e.getActionCommand())){
			if (obstacles!=null && startroot!=null && goalroot!=null){
				showpath=false;
				clearTrees();
				System.out.println("Start!");
				start.setEnabled(false);
				// Do the RRTrees algorithm here
				path = null;
				int k=0;
				while (path==null){
					if (k%2==0)
						path = RRTBidirectionalOneStep(startroot, goalroot);
					else
						path = RRTBidirectionalOneStep(goalroot, startroot);
					k++;
				}
				System.out.println(k+" iterations");
				start.setEnabled(true);
				showpathbutton.setEnabled(true);
				mulist.setEnabled(true);
			}
			else{
				System.out.println("Set start, goal, and obstacles first!");
			}
		}
		else if ("showpath".equals(e.getActionCommand())){
			if (path!=null){
				showpath = !showpath;
			}else{}
		}
		else if ("mulist".equals(e.getActionCommand())) {
			JComboBox cb = (JComboBox)e.getSource();
	        String s_value = (String)cb.getSelectedItem();
	        double value = new Double(s_value);
	        if (value != mu) {
	        	mu = value;
	        	showpathbutton.setEnabled(false);
	        	showpath = false;
	        	// reset trees
	        	RRTreeNode tempstart = new RRTreeNode();
	        	RRTreeNode tempgoal = new RRTreeNode();
	        	tempstart.position = (Point2D) startroot.position.clone();
	        	tempgoal.position = (Point2D) goalroot.position.clone();
	    		startroot = tempstart;
	    		goalroot = tempgoal;
	    		repaint();
	        	// Do the RRTrees algorithm here
				path = null;
				int k=0;
				while (path==null){
					if (k%2==0)
						path = RRTBidirectionalOneStep(startroot, goalroot);
					else
						path = RRTBidirectionalOneStep(goalroot, startroot);
					k++;
				}
				System.out.println(k+" iterations");
				start.setEnabled(true);
				showpathbutton.setEnabled(true);
				mulist.setEnabled(true);
	        }
		}
		else System.err.println("Unrecognized action command: " + e.getActionCommand());
		repaint();
	}
	
	private void clearTrees(){
		if (startroot!=null)
			startroot.children=new LinkedList<RRTreeNode>();
		if (goalroot!=null)
			goalroot.children=new LinkedList<RRTreeNode>();
	}
	
	private void loadStartGoalFile(String filename){
    	RRTreeNode tempstart = startroot;
    	RRTreeNode tempgoal = goalroot;
		File file = new File(filename);
    	Scanner scanner = null;
    	try {
    		scanner = new Scanner(file);
    	}
    	catch (FileNotFoundException e) {
    		System.out.println("File not found: " + filename);
    	}
    	try {
    		// first value, num obstacles
    		startroot = new RRTreeNode();
    		goalroot = new RRTreeNode();
    		double startx = scanner.nextDouble();
    		double starty = scanner.nextDouble();
    		startroot.position=new Point2D.Double(startx, starty);
    		double goalx = scanner.nextDouble();
    		double goaly = scanner.nextDouble();
    		goalroot.position=new Point2D.Double(goalx, goaly);
    		if (scanner.hasNextDouble())
    			throw new InputMismatchException();
    	}
    	catch (Exception e) {
    		System.out.println("Exception: " + e);
    		startroot = tempstart;
    		goalroot = tempgoal;
    		return;
    	}
    	scanner.close();
	}
	
	private void loadObstacleFile(String filename){
    	LinkedList<Obstacle> tempobstacles = obstacles;
    	int tempnumobstacles = num_obstacles;
    	File file = new File(filename);
    	Scanner scanner = null;
    	try {
    		scanner = new Scanner(file);
    	}
    	catch (FileNotFoundException e) {
    		System.out.println("File not found: " + filename);
    	}
    	try {
    		// first value, num obstacles
    		num_obstacles = scanner.nextInt();
    		// iter on obstacles
    		obstacles = new LinkedList<Obstacle>();
    		for (int obst = 0; obst < num_obstacles; obst++){
    			int num_vert = scanner.nextInt();
    			Obstacle ob = new Obstacle(num_vert);
    			// iter on vertices
    			for (int v = 0; v < num_vert; v++){
    				double cX = scanner.nextDouble();
    				double cY = scanner.nextDouble();
    				// add to obstacle
    				ob.addVertice(cX,cY);
    			}
    			ob.lock();
    			obstacles.add(ob);
    		}
    		Obstacle wall = new Obstacle(4);
    		wall.addVertice(0,0);
    		wall.addVertice(600,0);
    		wall.addVertice(600,600);
    		wall.addVertice(0,600);
    		wall.setWall(true);
    		wall.lock();
    		obstacles.add(wall);
    		num_obstacles++;
    	}
    	catch (Exception e) {
    		System.out.println("Exception: " + e);
    		obstacles = tempobstacles;
    		num_obstacles = tempnumobstacles;
    	}
    	scanner.close();
	}
	
	RRTreeNode NEAREST_NEIGHBOR(RRTreeNode target, RRTreeNode root){
		return NearestNeighborHelper(target,root,root);
	}
	
	RRTreeNode NearestNeighborHelper(RRTreeNode target, RRTreeNode current, RRTreeNode closest_so_far){
		
		if (distance(target,current)<distance(target,closest_so_far)){
			closest_so_far = current; 
		}else{}
		for (RRTreeNode child : current.children){
			RRTreeNode nearest_child = NearestNeighborHelper(target,child,closest_so_far);
			if (distance(target,nearest_child) < distance(target,closest_so_far)){
				closest_so_far = nearest_child;
			}else{}
		}
		return closest_so_far;
	}
	
	/**
	 * Returns the distance between two nodes.
	 * @param a Node a
	 * @param b Node b
	 * @return Distance between node a and node b.
	 */
	double distance(RRTreeNode a, RRTreeNode b){
		// Have yet to decide how orientation factors in distance
		return Math.sqrt(Math.pow(a.position.getX()-b.position.getX(),2)+
				         Math.pow(a.position.getY()-b.position.getY(),2));
	}
	
	private RRTreeNode RANDOM_STATE(){
		Random rand = new Random();
		Point2D rand_pos = new Point2D.Double(600*rand.nextDouble(),600*rand.nextDouble());
		double rand_or = 360*rand.nextDouble();
		RRTreeNode random_node = new RRTreeNode(null, null, rand_pos, rand_or);
		return random_node;
	}
	
	/**
	 * 
	 * @param root Root of a RRTree
	 * @param x_rand Node that we would like to expand in the direction of.
	 * @return Node that was added to the RRTree, that is mu in the direction of x_rand from the nearest point in the RRTree. Return is null if there is a collision. 
	 */
	private RRTreeNode EXTEND(RRTreeNode root, RRTreeNode x_rand){
		RRTreeNode x_near = NEAREST_NEIGHBOR(x_rand,root);
		return NEW_STATE(root, x_rand, x_near);
	}
	
	/**
	 * 
	 * @param root Root of a RRTree
	 * @param x_near Node that we would like to attach a node to
	 * @param x_new Node that we would like to attach to RRTree
	 * @return True if adding the node causes a collision with existing tree, false otherwise.
	 */
	private boolean collides(RRTreeNode x_near, RRTreeNode x_new){
		//not implemented yet
		//return collidesHelper(new Line2D.Double(x_near.position, x_new.position));
		for (Obstacle o : obstacles){
			if (o.intersect_ext(x_near.position.getX(),x_near.position.getY(),x_new.position.getX(),x_new.position.getY()))
				return true;
		}
		return false;
	}
	
	// actual collides implementation, check a line with each obstacle
	private boolean collidesHelper(Line2D newline){
		for (Obstacle o : obstacles){
			if (o.intersect_ext(newline.getX1(),newline.getY1(),newline.getX2(),newline.getY2()))
				return true;
		}
		return false;
	}
	
	
	/**
	 * 
	 * @param root Root of a RRTree
	 * @param x Node that we would like to expand in the direction of
	 * @param x_near Node that is nearest to x in the RRTree
	 * @return Node that was added to the RRTree, that is mu in the direction of x from x_near. Return is null if there is a collision. 
	 */
	private RRTreeNode NEW_STATE(RRTreeNode root, RRTreeNode x, RRTreeNode x_near){
		double angle = Math.atan2((x.position.getY()-x_near.position.getY()),(x.position.getX()-x_near.position.getX()));
		RRTreeNode x_new;
		if (distance(x,x_near)<mu)
			x_new = new RRTreeNode(x_near, new LinkedList<RRTreeNode>(),x.position,x.orientation);
		else
			x_new = new RRTreeNode(x_near, new LinkedList<RRTreeNode>(),new Point2D.Double(x_near.position.getX()+mu*Math.cos(angle),x_near.position.getY()+mu*Math.sin(angle)),mu_orientation);
		if (!collides(x_near, x_new)){
			x_near.children.add(x_new);
			return x_new;
		}
		else
			return null;
	}
	
	LinkedList <RRTreeNode> PATH(RRTreeNode T_a, RRTreeNode T_b, RRTreeNode x_a_new, RRTreeNode x_b_new){
		LinkedList <RRTreeNode> path = new LinkedList <RRTreeNode>();
		while (x_a_new!=null){
			path.addFirst(x_a_new);
			x_a_new = x_a_new.parent;
		}
		while (x_b_new!=null){
			path.addLast(x_b_new);
			x_b_new = x_b_new.parent; 
		}
		return path;
	}
	
	
	/**
	 * 
	 * @param k Number of iterations
	 */
	public LinkedList <RRTreeNode>RRTBidirectionalOneStep(RRTreeNode T_a, RRTreeNode T_b){
		
		
		RRTreeNode x_rand = RANDOM_STATE();
		System.out.println("x_rand="+x_rand);
		RRTreeNode x_a_new = EXTEND(T_a,x_rand); // EXTEND returns null if unsuccessful
		System.out.println("x_a_new="+x_a_new);
		if (x_a_new!=null){
			RRTreeNode x_b_new = EXTEND(T_b,x_a_new);
			if (x_b_new == null){
				System.out.println("Tree b could not be extended");
			}
			else if (x_b_new.equals(x_a_new)){
				System.out.println("The trees met");
				return PATH(T_a,T_b, x_a_new, x_b_new);
			}
			else{
				System.out.println("The trees extended towards each other");
			}
		}
		else{
			System.out.println("Collision");
		}
		
		return null;
	}
	
	public static void main(String args[]){
		
		RRTree rrtree = new RRTree();
		
	}
	
}
